function [J, J1, J2] = cal_Jacobian(q, t1, t2)
l1 = 1; l2 = 1;
J = [-l1 * sin(q(1)) - l2 * sin(q(1) + q(2)), -l2 * sin(q(1) + q(2));
    0, 0;
    l1 * cos(q(1)) + l2 * cos(q(1) + q(2)), l2 * cos(q(1) + q(2))];
J1 = [-t1 * sin(q(1)), 0;
    0, 0;
    l1 * cos(q(1)), 0];
J2 = [-l1 * sin(q(1)) - t2 * sin(q(1) + q(2)), -t2 * sin(q(1) + q(2));
    0, 0;
    l1 * cos(q(1)) + t2 * cos(q(1) + q(2)), t2 * cos(q(1) + q(2))];
